// --------------------------------------------------------------------------------------------------------------------
// <copyright file="KukaLBR4IK6DOFClosedForm.cs" company="Microsoft Corporation">
// The MIT License (MIT)
// 
// Copyright (c) 2014, Microsoft Corporation
// 
// Permission is hereby granted, free of charge, to any person obtaining a copy
//  of this software and associated documentation files (the "Software"), to deal
//  in the Software without restriction, including without limitation the rights
//  to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
//  copies of the Software, and to permit persons to whom the Software is
//  furnished to do so, subject to the following conditions:
// 
// The above copyright notice and this permission notice shall be included in
//  all copies or substantial portions of the Software.
// 
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
//  IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
//  FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
//  AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
//  LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
//  OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
//  THE SOFTWARE.
// </copyright>
// --------------------------------------------------------------------------------------------------------------------

namespace Microsoft.Robotics.Hardware.KukaLBR
{
    using System;
    using System.Collections.Generic;
    using Microsoft.Robotics.Kinematics;
    using Microsoft.Robotics.Numerics;

    using ShoNS.Array;

    /// <summary>
    /// A class that does the IK according to KUKA's document
    /// A modified printout with hand-written modifications can
    /// be retrieved at the URL after the break.
    /// </summary>
    /// The original document: http://www.kuka-robotics.com/usa/en/products/software/educational_framework/arm_tutorials/PS_Content_Arm2.htm
    /// Implementation document: http://sharepoint/sites/Baker/Shared%20Documents/Reference%20documents/Manipulation%20Pipeline/kinematics_derivition.pdf
    public class KukaLBR4IK6DOFClosedForm
    {
        /// <summary>
        /// Length between joint0 and joint1
        /// </summary>
        public const double L01 = 310.5 / 1000.0;

        /// <summary>
        /// Length between joint2 and joint3
        /// </summary>
        public const double L23 = 400 / 1000.0;

        /// <summary>
        /// Length between joint3 and joint4
        /// </summary>
        public const double L34 = 390 / 1000.0;

        /// <summary>
        /// Length between joint6 and end effector
        /// </summary>
        public const double L6E = (77.5 + 95) / 1000.0;

        /// <summary>
        /// Number of Degree of freedom
        /// </summary>
        public const int NumberOfDOFs = 6;

        /// <summary>
        /// Number of possible solutions according to the
        /// Kuka document
        /// </summary>
        public const int NumberOfSolutions = 8;

        /// <summary>
        /// Do the inverse kinematics calculation
        /// </summary>
        /// <param name="kc">A kinematic chain that can be used for temporary forward kinematics</param>
        /// <param name="endEffectorPose">The goal pose of the end effector</param>
        /// <returns>The joint angles to reach the goal pose. There are eight possible configurations in general. Please
        /// refer to the document for your preference.</returns>
        /// Implementation document: http://sharepoint/sites/Baker/Shared%20Documents/Reference%20documents/Manipulation%20Pipeline/kinematics_derivition.pdf
        public static List<DoubleArray> DedicatedIK(KinematicChain kc, Matrix4 endEffectorPose)
        {
            // The solutions are in column vectors
            List<DoubleArray> solutions = new List<DoubleArray>(KukaLBR4IK6DOFClosedForm.NumberOfSolutions);
            for (int i = 0; i < KukaLBR4IK6DOFClosedForm.NumberOfSolutions; ++i)
            {
                solutions.Add(new DoubleArray(KukaLBR4IK6DOFClosedForm.NumberOfDOFs, 1));
            }

            Vector3 positionOfK6InK0 = new Vector3(endEffectorPose[0, 3], endEffectorPose[1, 3], endEffectorPose[2, 3]);
            Vector3 axisZOfK6InK0 = new Vector3(endEffectorPose[0, 2], endEffectorPose[1, 2], endEffectorPose[2, 2]);

            // Step1: compute joint1
            // first four solutions can be mirrored and generate the second four solutions
            Vector3 p04 = positionOfK6InK0 - axisZOfK6InK0 * KukaLBR4IK6DOFClosedForm.L6E;
            solutions[0][0] = solutions[1][0] = solutions[2][0] = solutions[3][0] = Math.Atan2(p04.Y, p04.X);
            solutions[4][0] = solutions[5][0] = solutions[6][0] = solutions[7][0] = Math.Atan2(p04.Y, p04.X) + Math.PI;

            // Step2: compute joint2
            Vector3 p01 = new Vector3(0, 0, KukaLBR4IK6DOFClosedForm.L01);
            Vector3 p14 = p04 - p01;

            // use first solution for joint1 as a reference
            Matrix4[] transforms = kc.GetJointTransforms(solutions[0]);
            Matrix4 t01 = transforms[0];
            Quaternion rot01 = new Quaternion(t01);
            Matrix4 t14InK1 = HomogenousTransform.CreateFromQuaternion(Quaternion.Inverse(rot01)) * HomogenousTransform.CreateFromTranslation(p14); // p14 is relative, projection is enough
            Vector3 p14InK1 = new Vector3(t14InK1[0, 3], t14InK1[1, 3], t14InK1[2, 3]);
            double beta1 = Math.Atan2(p14InK1.Z, p14InK1.X);

            // very small numerical perturbation will have this inner product be a tiny little bit above 1 or below -1
            double valueForBeta2 = ((KukaLBR4IK6DOFClosedForm.L23 * KukaLBR4IK6DOFClosedForm.L23) + (p14InK1.Length() * p14InK1.Length()) - KukaLBR4IK6DOFClosedForm.L34 * KukaLBR4IK6DOFClosedForm.L34) / (2 * KukaLBR4IK6DOFClosedForm.L23 * p14InK1.Length());
            double beta2 = Math.Acos(Math.Abs(valueForBeta2) > 1 ? 1 * Math.Sign(valueForBeta2) : valueForBeta2);
            solutions[0][1] = solutions[1][1] = (beta1 + beta2);
            solutions[2][1] = solutions[3][1] = (beta1 - beta2);

            /* initial pose of the arm after joint2 is horizontal,
             * that's the alternative solution is Math.PI - *,
             * where * refers to the solutions from above
             */
            solutions[4][1] = solutions[5][1] = Math.PI - (beta1 + beta2);
            solutions[6][1] = solutions[7][1] = Math.PI - (beta1 - beta2);

            // Step3: compute joint3
            double valueForPhi = ((KukaLBR4IK6DOFClosedForm.L23 * KukaLBR4IK6DOFClosedForm.L23) + (KukaLBR4IK6DOFClosedForm.L34 * KukaLBR4IK6DOFClosedForm.L34) - (p14.Length() * p14.Length())) / (2 * KukaLBR4IK6DOFClosedForm.L23 * KukaLBR4IK6DOFClosedForm.L34);
            double phi = Math.Acos(Math.Abs(valueForPhi) > 1 ? 1 * Math.Sign(valueForPhi) : valueForPhi);
            solutions[0][2] = solutions[1][2] = (Math.PI - phi);
            solutions[2][2] = solutions[3][2] = (Math.PI + phi);
            solutions[4][2] = solutions[5][2] = -(Math.PI - phi);
            solutions[6][2] = solutions[7][2] = -(Math.PI + phi);

            // Step4: compute joint5
            // There are two different solutions for the z-axis of joint4
            // resulted from the solutions for joint1, joint2, and joint3.
            // The location for joint4 is the same for all the solutions
            // Solutions 0,1,4,5 produce one potential z-axis and
            // solutions 2,3,6,7 produce the other one
            Matrix4[] transforms0 = kc.GetJointTransforms(solutions[0]);
            Matrix4[] transforms2 = kc.GetJointTransforms(solutions[2]);

            // based on the first z-axis of joint4
            Matrix4 transformK4A = transforms0[3];
            Vector3 axisZOfK4InK0A = new Vector3(transformK4A[0, 2], transformK4A[1, 2], transformK4A[2, 2]);
            double dotValueA = Vector3.Dot(axisZOfK4InK0A, axisZOfK6InK0);
            solutions[0][4] = solutions[4][4] = Math.Acos(Math.Abs(dotValueA) > 1 ? 1 * Math.Sign(dotValueA) : dotValueA);
            solutions[1][4] = solutions[5][4] = -solutions[0][4];

            // based on the second z-axis of joint4
            Matrix4 transformK4B = transforms2[3];
            Vector3 axisZOfK4InK0B = new Vector3(transformK4B[0, 2], transformK4B[1, 2], transformK4B[2, 2]);
            double dotValueB = Vector3.Dot(axisZOfK4InK0B, axisZOfK6InK0);
            solutions[2][4] = solutions[6][4] = Math.Acos(Math.Abs(dotValueB) > 1 ? 1 * Math.Sign(dotValueB) : dotValueB);
            solutions[3][4] = solutions[7][4] = -solutions[2][4];

            // Step5: compute joint4 and joint6
            // Step5.1: two different poses for joint 4
            Matrix4 transformInK4A = HomogenousTransform.Inverse(transformK4A) * endEffectorPose;
            Matrix4 transformInK4B = HomogenousTransform.Inverse(transformK4B) * endEffectorPose;

            // Step5.2: compute joint4
            solutions[0][3] = solutions[5][3] = Math.Atan2(transformInK4A[1, 2], transformInK4A[0, 2]) + Math.PI;
            solutions[1][3] = solutions[4][3] = Math.Atan2(transformInK4A[1, 2], transformInK4A[0, 2]);
            solutions[2][3] = solutions[7][3] = Math.Atan2(transformInK4B[1, 2], transformInK4B[0, 2]) + Math.PI;
            solutions[3][3] = solutions[6][3] = Math.Atan2(transformInK4B[1, 2], transformInK4B[0, 2]);

            // Step5.3: compute joint6
            solutions[0][5] = solutions[4][5] = Math.Atan2(-transformInK4A[2, 1], transformInK4A[2, 0]);
            solutions[1][5] = solutions[5][5] = Math.Atan2(-transformInK4A[2, 1], transformInK4A[2, 0]) + Math.PI;
            solutions[2][5] = solutions[6][5] = Math.Atan2(-transformInK4B[2, 1], transformInK4B[2, 0]);
            solutions[3][5] = solutions[7][5] = Math.Atan2(-transformInK4B[2, 1], transformInK4B[2, 0]) + Math.PI;

            // resolve joint4 and joint6 when joint5 is zero
            ResolveJoint4AndJoint6(kc, solutions, endEffectorPose);

            // reorganize all the joints so that they all fall into [-pi,pi] range
            for (int i = 0; i < KukaLBR4IK6DOFClosedForm.NumberOfSolutions; ++i)
            {
                InverseKinematicsNumerical.NormalizeSolutionAndCheckWithJointLimits(solutions[i], null);
            }

            return solutions;
        }

        /// <summary>
        /// correct joint4 and joint6 when joint5 is zero
        /// </summary>
        /// <param name="kc">kinematic chain</param>
        /// <param name="solutions">solutions found</param>
        /// <param name="endEffectorPose">end effector pose</param>
        public static void ResolveJoint4AndJoint6(KinematicChain kc, List<DoubleArray> solutions, Matrix4 endEffectorPose)
        {
            double smallNumber = MathConstants.ErrorEpsilon;

            for (int i = 0; i < KukaLBR4IK6DOFClosedForm.NumberOfSolutions; ++i)
            {
                if (Math.Abs(solutions[i][4, 0]) < smallNumber)
                {
                    // according to our DH parameter and our coordinate system
                    // x-axis of joint3 should be aligned with the x-axis of joint6
                    // their angle should be introduced by the rotation of joint4
                    // and joint6
                    Matrix4[] transforms = kc.GetJointTransforms(solutions[i]);
                    Vector3 axisXOfJoint3 = new Vector3(transforms[2][0, 0], transforms[2][1, 0], transforms[2][2, 0]);
                    Vector3 axisXOfJoint6 = new Vector3(endEffectorPose[0, 0], endEffectorPose[1, 0], endEffectorPose[2, 0]);
                    Vector3 axisZOfJoint6 = new Vector3(endEffectorPose[0, 2], endEffectorPose[1, 2], endEffectorPose[2, 2]);
                    
                    // we need to decide the direction of the angle
                    int sign = Math.Sign(Vector3.Dot(Vector3.Cross(axisXOfJoint3, axisXOfJoint6), axisZOfJoint6));
                    solutions[i][3, 0] = Math.Acos(Vector3.Dot(axisXOfJoint3, axisXOfJoint6)) * sign;
                    solutions[i][5, 0] = 0;
                    solutions[i][4, 0] = 0;
                }
            }
        }
    }
}
